import DobotDllType as dType
import threading
import numpy as np

CON_STR = {
    dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
    dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
    dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"}

# 将dll读取到内存中并获取对应的CDLL实例
api = dType.load()
# 建立与dobot的连接
state = dType.ConnectDobot(api, "", 115200)[0]
print("Connect status:", CON_STR[state])

pearl_local = np.array([[198.7, -24.5],
                        [204.5, -24.5],
                        [204.5, -34.5],
                        [212, -33]])

dobot_center = np.array([137, -16.5])
box_local = np.array([-7, -163])

if state == dType.DobotConnect.DobotConnect_NoError:
    # 清空队列
    dType.SetQueuedCmdClear(api)

    # 设置运动参数
    dType.SetHOMEParams(api, 200, 200, 200, 200, isQueued=1)
    dType.SetPTPJumpParams(api, 30, 100, isQueued=1)
    dType.SetPTPCommonParams(api, 100, 100, isQueued=1)
    # dType.SetEndEffectorParams(api, 0, 0, 60, isQueued=1)    #设置气泵的状态

    # 回零
    dType.SetHOMECmd(api, temp=0, isQueued=1)

    # # 设置ptpcmd内容并将命令发送给dobot
    Index_0 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                              dobot_center[0], dobot_center[1],
                              20, 40, isQueued=1)[0]

    dType.dSleep(1000)
    for dobot_x, dobot_y in pearl_local:
        Index_1 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                                  dobot_x, dobot_y, 0,10, isQueued=1)[0]
        dType.dSleep(1000)
        Index_2 = dType.SetEndEffectorSuctionCup(api, 1, 1, isQueued=1)[0]

        Index_3 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                                  box_local[0], box_local[1], 0, 10, isQueued=1)[0]
        dType.dSleep(1000)
        Index_4 = dType.SetEndEffectorSuctionCup(api, 0, 1, isQueued=1)[0]
        print(dobot_x, dobot_y)

    Index_5 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                              dobot_center[0], dobot_center[1],
                              20, 40, isQueued=1)[0]

    # 开始执行指令队列
    print('开始')
    dType.SetQueuedCmdStartExec(api)

    # 如果还未完成指令队列则等待
    while Index_5 > dType.GetQueuedCmdCurrentIndex(api)[0]:
        dType.dSleep(100)

    print(0)
    # 停止执行指令
    dType.SetQueuedCmdStopExec(api)
# 断开连接
dType.DisconnectDobot(api)
